#include <QCoreApplication>
#include <QDebug>
#include "jointdecouple.h"

#define robotAxisesCount 4

QVector<JointAttribute> robotJoints;
QVector<MotorData>  motorFeedback; //电机反馈值
int jointType[robotAxisesCount] = {0, 0, 1, 0};  //第３轴为平动关节，其余为旋转关节
int coupleFlag[robotAxisesCount] = {1, 0, 1, 0}; //第１和第３轴分别收到第２和第４轴耦合
int gearRatio[robotAxisesCount] = {80, 50, 2, 10};
int screwPitch[robotAxisesCount] = {0, 0, 10, 0};
int encoderResolution[robotAxisesCount] = {17, 17, 17, 17};
int coupleJoint[robotAxisesCount] = {1, 0, 3, 0}; //对第１和第３轴造成耦合的轴号，从０开始
int coupleReducer[robotAxisesCount] = {1, 0, 1, 0};

int robotInit()
{
    for(int i=0;i<robotAxisesCount;i++)
    {
         JointAttribute joint;
         joint.externAxisFlag = 0;
         //joint.massCenter.resize(3);
         //joint.inertiaMatrix.resize(6);

         joint.jointType = jointType[i];
         joint.coupleFlag = coupleFlag[i];
         joint.gearRatio = gearRatio[i];
         joint.screwPitch = screwPitch[i];
         joint.encoderResolution = encoderResolution[i];
         joint.coupleJoint = coupleJoint[i];
         joint.coupleReducer = coupleReducer[i];

         joint.pulsePerRound = 1 << joint.encoderResolution;

         robotJoints.append(joint);
    }
}

int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    //给定关节数据
//    float posittion[robotAxisesCount] = {-41.577/180*M_PI, 102.404/180*M_PI, 0.440/1000, -26.177/180*M_PI};
//    float posittion[robotAxisesCount] = {-67.462/180*M_PI, 86.650/180*M_PI, -4.598/1000, 3.928/180*M_PI};
    float posittion[robotAxisesCount] = {-41.671/180*M_PI, 105.165/180*M_PI, 0.166/1000, -26.177/180*M_PI};
    float vel[robotAxisesCount] = {20.1/180*M_PI, 20.1/180*M_PI, 900.5/1000, 20.1/180*M_PI};
    float acc[robotAxisesCount] = {20.1/180*M_PI, 20.1/180*M_PI, 1200.5/1000, 20.1/180*M_PI};

    JointDecouple* jointDecouple;//关节解耦
    robotInit();
    motorFeedback.resize(robotAxisesCount);
    jointDecouple = new JointDecouple(&robotJoints, &motorFeedback);

    JntArray jointsPosition(robotAxisesCount);
    JntArray jointsVel(robotAxisesCount);
    JntArray jointAcc(robotAxisesCount);
    for (int i = 0; i < robotAxisesCount; i++)
    {
        jointsPosition(i) = posittion[i];
        jointsVel(i) = vel[i];
        jointAcc(i) = acc[i];
    }

    for (int i = 0; i < robotAxisesCount; i++)
    {
        qDebug() << "joint[" << i << "]:" << "posision=" << jointsPosition(i) <<",vel=" << jointsVel(i) << ",acc=" << jointAcc(i);
    }

    jointDecouple->decouple(jointsPosition, jointsVel, jointAcc);

    qDebug() << "---decouple---";
    for (int i = 0; i < robotAxisesCount; i++)
    {
        qDebug() << "joint[" << i << "]:" << "posision=" << jointDecouple->position(i) <<",vel=" << jointDecouple->vel(i) << ",acc=" << jointDecouple->acc(i);
    }

    //关节角转换为电机脉冲
    for(int i=0; i<robotAxisesCount; i++)
    {
        if( robotJoints[i].jointType==0 )//旋转关节
        {
            //电机编码器值＝（关节角度／３６０）×减速比× 电机每圈的脉冲数
            motorFeedback[i].position = jointDecouple->position(i) * robotJoints[i].gearRatio /360.0  * robotJoints[i].pulsePerRound;
            //电机速度＝（关节角度速度／３６０）×减速比× 电机每圈的脉冲数
            motorFeedback[i].speed = jointDecouple->vel(i) * robotJoints[i].gearRatio /360.0 * robotJoints[i].pulsePerRound;
        }
        else//移动关节
        {
            //电机编码器值＝关节位移／螺距 ×减速比× 电机每圈的脉冲数
            motorFeedback[i].position = jointDecouple->position(i) * robotJoints[i].gearRatio / robotJoints[i].screwPitch  * robotJoints[i].pulsePerRound;
            //电机速度＝关节位移速度／螺距 ×减速比× 电机每圈的脉冲数
            motorFeedback[i].speed = jointDecouple->vel(i) * robotJoints[i].gearRatio / robotJoints[i].screwPitch * robotJoints[i].pulsePerRound;
        }
    }

    jointDecouple->reverseDecouple();

    qDebug() << "---reverse_decouple---";
    for (int i = 0; i < robotAxisesCount; i++)
    {
        qDebug() << "joint[" << i << "]:" << "posision=" << jointDecouple->positionFeedback(i) <<",vel=" << jointDecouple->velFeedback(i);
    }

    return a.exec();
}
